#include "drivetrain.h"

#define EPSLON 0.01 // error tolerance
#define MAX_TIME_ALLOWED 30 // default maximum time to reach waypt

#define CLAMP(x, UB, LB) ( (x>UB)?(UB):((x<LB)?(LB):(x)) )


/********************************************************************
   Function Name: reset 
   Parameters: none
   Return: none
   Description: reset the drivetrain
*********************************************************************/
void drivetrain::reset()
{
   // stop the drivetrain
   stop();   

   // get the next waypoint
   if( traj.size() > 0 )
   {
      nextWaypt = get_next_waypt();
   }
} /*--- end of function ---*/

/********************************************************************
   Function Name: getMode 
   Parameters: none
   Return: Mode
   Description: return the current operating mode of the robot
*********************************************************************/
DriveTrainMode_t drivetrain::getMode()
{
   return Mode;
}


/********************************************************************
   Function Name: start
   Parameters: none
   Return: No_Err
           FAIL_TO_START
   Description: start the drivetrain
*********************************************************************/
ErrCode_t drivetrain::start()
{
   ErrCode_t ErrCode = NO_ERR; 
   Mode = RUNNING;
   return ErrCode;
} /*--- end of function ---*/

/********************************************************************
   Function Name: stop
   Parameters: none
   Return: none
   Description: stop the drivetrain
*********************************************************************/
void drivetrain::stop()
{
   Mode = PAUSED;
} /*--- end of function ---*/

/********************************************************************
   Function Name: set_traj
   Parameters: waypt_t* nWaypt - array of waypoints
               int size        - number of waypoints to be added
               bool reset      - 1: clear the current traj and 
                                    add new waypoints
                                 2: add new waypoints to the
                                    end of current traj
   Return:                     - NO_ERR
                               - TRAJ_SIZE_EXCEEDS_LIMIT
   Description: Set the traj for the robot to follow.
*********************************************************************/
ErrCode_t drivetrain::set_traj(waypt_t* nWaypt, int size, bool reset=false)
{
   ErrCode_t ErrCode = NO_ERR;

   // clear the traj if 'reset' is true
   if( reset == true )
   {
      traj.clear();
   }

   // adjust the traj
   if( traj.size() + size > MAX_TRAJ_SIZE )
   { // reject the request if the size of current traj gets too large
      ErrCode = TRAJ_SIZE_EXCEEDS_LIMIT;
   }
   else // otherwise
   {
      for(int i=0; i<size; i++)
      {
         traj.push_back(*nWaypt++);
      }
   }
   return ErrCode;
} /*--- end of function ---*/

/********************************************************************
   Function Name: get_next_waypt
   Parameters:    none
   Return:        waypt_t
   Description:   return the next waypt of the trajectory
*********************************************************************/
waypt_t drivetrain::get_next_waypt()
{
   waypt_t next_waypt;
   next_waypt = traj.front();
   traj.erase(traj.begin());
   return next_waypt;
} /*--- end of function ---*/

/********************************************************************
   Function Name: display_traj
   Parameters:    none
   Return:        none
   Description:   print the current traj for debugging purpose
*********************************************************************/
void drivetrain::display_traj()
{
   std::cout << "current traj info\n";
   std::cout << "number of waypoints: " << traj.size() << std::endl;
   for(int i =0; i<traj.size(); i++)
   {
      std::cout << "wpt " << i << " : x = " << traj[i].pos.x;
      std::cout << " y = " << traj[i].pos.y;
      std::cout << " heading = " << traj[i].heading;
      std::cout << " Tstamp = " << traj[i].Tstamp << std::endl;
   }
   std::cout << std::endl;
}/*--- end of function ---*/


/********************************************************************
   Function Name: vehicle_posUpdate
   Parameters: const WarbotsMsg::velocityMsg& msg
   Return: NULL
   Description: update the location of the robot according to the
                camera feedback
*********************************************************************/
void drivetrain::vehicle_posUpdate(const WarbotsMsg::posUpdateMsg& msg)
{
   rState.pos.x = msg.x;
   rState.pos.y = msg.y;
   rState.heading = msg.theta;
   if( traj.size() > 0 && atWaypt() )
   {
      nextWaypt = get_next_waypt();
   }
   //ROS_INFO("DT: r_pos [%f], [%f], [%f]\n", rState.pos.x, rState.pos.y, rState.heading); 
} /*--- end of function ---*/

/********************************************************************
   Function Name: get_wheelRotSpd
   Parameters: none
   Return: none
   Description: an array of four elements containing the speed of each wheel
*********************************************************************/
double* drivetrain::get_wheelRotSpd()
{
   return rParam.rRotSpd;
} /*--- end of function ---*/

/********************************************************************
   Function Name: atWaypt
   Parameters: none
   Return: True  - the robot has reached the waypoint
           False - the robot has not reached the waypoint yet
   Description: check if the robot has reached the waypoint
*********************************************************************/
bool drivetrain::atWaypt()
{
   double delta_x, delta_y, delta_theta;

   delta_x = rState.pos.x-nextWaypt.pos.x;
   delta_y = rState.pos.y-nextWaypt.pos.y;
   delta_theta = rState.heading-nextWaypt.heading;

   if(fabs(delta_x)>EPSLON || fabs(delta_y)>EPSLON || fabs(delta_theta)>EPSLON)
   {
       return false;
   }
   return true;
}

/********************************************************************
   Function Name: set_wheelRotSpd
   Parameters: none
   Return: none
   Description: calculate and set the rotation speed of each omni wheel
*********************************************************************/
void drivetrain::set_wheelRotSpd(double* set_spd)
{
   double posV[3], spd[3], spd_l[3];
   double sAlpha1, sAlpha2, sAlpha3, sAlpha4;
   double cAlpha1, cAlpha2, cAlpha3, cAlpha4;
   double rWheel = rParam.rWheels;
   double dt = 1.0/DRIVE_CTRL_LOOP_RATE;
   set_spd[0] = set_spd[1] = set_spd[2] = set_spd[3] = 0;


   if( !atWaypt() )
   {
      posV[0] = nextWaypt.pos.x - rState.pos.x;
      posV[1] = nextWaypt.pos.y - rState.pos.y;
      posV[2] = nextWaypt.heading - rState.heading;

      if(abs(nextWaypt.Tstamp) < EPSLON )
      {
         nextWaypt.Tstamp = MAX_TIME_ALLOWED;
      }
   
      for(int i=0; i<3; i++)
      {
         spd[i] = posV[i]/nextWaypt.Tstamp;
      }

      spd_l[0] = cos(rState.heading)*spd[0] + sin(rState.heading)*spd[1];
      spd_l[1] = -sin(rState.heading)*spd[0] + cos(rState.heading)*spd[1];
      spd_l[2] = spd[2];

      sAlpha1 = sin(rParam.alpha[0]);
      sAlpha2 = sin(rParam.alpha[1]);
      sAlpha3 = sin(rParam.alpha[2]);
      sAlpha4 = sin(rParam.alpha[3]);
      cAlpha1 = cos(rParam.alpha[0]);
      cAlpha2 = cos(rParam.alpha[1]);
      cAlpha3 = cos(rParam.alpha[2]);
      cAlpha4 = cos(rParam.alpha[3]);
    
      set_spd[0] = ((-1.0/sAlpha1)*spd_l[0] + (1.0/cAlpha1)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
      set_spd[1] = ((-1.0/sAlpha2)*spd_l[0] + (1.0/cAlpha2)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
      set_spd[2] = ((-1.0/sAlpha3)*spd_l[0] + (1.0/cAlpha3)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
      set_spd[3] = ((-1.0/sAlpha4)*spd_l[0] + (1.0/cAlpha4)*spd_l[1] + rParam.l*spd_l[2])/rWheel;

      nextWaypt.Tstamp -= dt;
      nextWaypt.Tstamp = CLAMP(nextWaypt.Tstamp, 0, MAX_TIME_ALLOWED);
   }
   
} /*--- end of function ---*/

/********************************************************************
   Function Name: manual_control
   Parameters: const WarbotsMsg::manualCtrlMsg
   Return: none
   Description: control the bot manually given position vector
*********************************************************************/
void drivetrain::manual_control(const WarbotsMsg::manualCtrlMsg& msg, double* set_spd)
{
   double posV[3], spd[3], spd_l[3];
   double sAlpha1, sAlpha2, sAlpha3, sAlpha4;
   double cAlpha1, cAlpha2, cAlpha3, cAlpha4;
   double rWheel = rParam.rWheels;
   double dt = 1.0/DRIVE_CTRL_LOOP_RATE;
   set_spd[0] = set_spd[1] = set_spd[2] = set_spd[3] = 0;

   posV[0] = (msg.mag)*cos(msg.ang);
   posV[1] = (msg.mag)*sin(msg.ang);
   posV[2] = 0.0;
   
   for(int i=0; i<3; i++)
   {
      spd[i] = posV[i]/MAX_TIME_ALLOWED;
   }

   spd_l[0] = spd[0];
   spd_l[1] = spd[1];
   spd_l[2] = spd[2];

   sAlpha1 = sin(rParam.alpha[0]);
   sAlpha2 = sin(rParam.alpha[1]);
   sAlpha3 = sin(rParam.alpha[2]);
   sAlpha4 = sin(rParam.alpha[3]);
   cAlpha1 = cos(rParam.alpha[0]);
   cAlpha2 = cos(rParam.alpha[1]);
   cAlpha3 = cos(rParam.alpha[2]);
   cAlpha4 = cos(rParam.alpha[3]);
    
   set_spd[0] = ((-1.0/sAlpha1)*spd_l[0] + (1.0/cAlpha1)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
   set_spd[1] = ((-1.0/sAlpha2)*spd_l[0] + (1.0/cAlpha2)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
   set_spd[2] = ((-1.0/sAlpha3)*spd_l[0] + (1.0/cAlpha3)*spd_l[1] + rParam.l*spd_l[2])/rWheel;
   set_spd[3] = ((-1.0/sAlpha4)*spd_l[0] + (1.0/cAlpha4)*spd_l[1] + rParam.l*spd_l[2])/rWheel;

} /*--- end of function ---*/

